#include "ultrasonic_driver.h"

namespace PandaUGV
{
    UltrasonicDriver::UltrasonicDriver()
    {
        ros::NodeHandle nh;
        ros::NodeHandle private_nh("~");
        std::string us_port;
        private_nh.param<std::string>("us_port", us_port, std::string("ttyUSB0"));

        us_l_publisher = nh.advertise<sensor_msgs::Range>("/ultrasonic_l", 50, true);
        us_c1_publisher = nh.advertise<sensor_msgs::Range>("/ultrasonic_c1", 50, true);
        us_c2_publisher = nh.advertise<sensor_msgs::Range>("/ultrasonic_c2", 50, true);
        us_r_publisher = nh.advertise<sensor_msgs::Range>("/ultrasonic_r", 50, true);

        sensor_msg_init(range_left, "camera");
        sensor_msg_init(range_center1, "camera");
        sensor_msg_init(range_center2, "camera");
        sensor_msg_init(range_right, "camera");

        this->bFinished = true;
        us_port = "/dev/" + us_port;
        connect(us_port, B9600);
        ROS_INFO("open the serial port: %s", us_port.c_str());
    }

    UltrasonicDriver::~UltrasonicDriver()
    {
        port_->closePort();
        port_ = NULL;
    }

    void UltrasonicDriver::processRxData(uint8_t data)
    {
        // #ifdef ULTRASONIC_DEBUG
            printf("%02X ", data);
        // #endif
        static int data_num = 0;
        static uint8_t checksum = 0;
        static uint8_t state = ultrasonic_status_enum::US_HEADER;
        switch (state)
        {
        case ultrasonic_status_enum::US_HEADER:
        {
            if (data_num == 0 && data == 0xFF)
            {
                state = ultrasonic_status_enum::US_DATA1_HIGH;
                checksum += data;
                data_num++;
            }
            break;
        }
        case ultrasonic_status_enum::US_DATA1_HIGH:
        {
            distance_data[0] = static_cast<uint16_t>(data) << 8;
            state = ultrasonic_status_enum::US_DATA1_LOW;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_DATA1_LOW:
        {
            distance_data[0] |= static_cast<uint16_t>(data);
            state = ultrasonic_status_enum::US_DATA2_HIGH;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_DATA2_HIGH:
        {
            distance_data[1] = static_cast<uint16_t>(data) << 8;
            state = ultrasonic_status_enum::US_DATA2_LOW;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_DATA2_LOW:
        {
            distance_data[1] |= static_cast<uint16_t>(data);
            state = ultrasonic_status_enum::US_DATA3_HIGH;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_DATA3_HIGH:
        {
            distance_data[2] = static_cast<uint16_t>(data) << 8;
            state = ultrasonic_status_enum::US_DATA3_LOW;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_DATA3_LOW:
        {
            distance_data[2] |= static_cast<uint16_t>(data);
            state = ultrasonic_status_enum::US_DATA4_HIGH;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_DATA4_HIGH:
        {
            distance_data[3] = static_cast<uint16_t>(data) << 8;
            state = ultrasonic_status_enum::US_DATA4_LOW;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_DATA4_LOW:
        {
            distance_data[3] |= static_cast<uint16_t>(data);
            state = ultrasonic_status_enum::US_CHECKSUM;
            checksum += data;
            data_num++;
            break;
        }
        case ultrasonic_status_enum::US_CHECKSUM:
        {
            if (checksum == data)
            {
                state = ultrasonic_status_enum::US_HEADER;
                this->bFinished = true;
                #ifdef ULTRASONIC_DEBUG
                    printf("\nchecksum=%02X\n", checksum);
                    printf("distance_data[0] = %d\n", distance_data[0]);
                #endif
                range_left.range = distance_data[3] / 1000.0;
                range_center1.range = distance_data[0] / 1000.0;
                range_center2.range = distance_data[1] / 1000.0;
                range_right.range = distance_data[2] / 1000.0;

                ros::Time timeStamp = ros::Time::now();
                range_left.header.stamp = timeStamp;
                range_center1.header.stamp = timeStamp;
                range_center2.header.stamp = timeStamp;
                range_right.header.stamp = timeStamp;

                us_l_publisher.publish(range_left);
                us_c1_publisher.publish(range_center1);
                us_c2_publisher.publish(range_center2);
                us_r_publisher.publish(range_right);

                checksum = 0;
                data_num = 0;
            }
            break;
        }
        }
    }

    void UltrasonicDriver::readData()
    {
        uint8_t rx_data = 0;
        std::cout << "read start" << std::endl;
        while (ros::ok())
        {
            std::cout << "read start 0" << std::endl;
            auto len = port_->readByte(&rx_data);
            std::cout << "len: " << len << std::endl;
            if (len < 1)
                continue;
            processRxData(rx_data);
        }
    }

    void UltrasonicDriver::sendData()
    {
        ros::Rate loop_rate(10);
        ros::Time begin = ros::Time::now();
        while (ros::ok())
        {
            ros::Duration duration = ros::Time::now() - begin;
            if (this->bFinished || duration.toSec() > 0.2f)
            {
                begin = ros::Time::now();
                this->bFinished = false;
                uint8_t data[1] = {0xFF};
                port_->writeData(data, 1);
            }
            loop_rate.sleep();
        }
    }

    void UltrasonicDriver::connect(std::string dev_name, uint32_t bouadrate)
    {
        port_ = std::shared_ptr<PandaUGV::SerialPort>(new PandaUGV::SerialPort(dev_name, bouadrate));
        if (port_->openPort() == 0)
        {
            read_data_thread_ = std::shared_ptr<std::thread>(new std::thread(std::bind(&UltrasonicDriver::readData, this)));
            send_data_thread_ = std::shared_ptr<std::thread>(new std::thread(std::bind(&UltrasonicDriver::sendData, this)));
        }
        else
        {
            ROS_ERROR("Failed to open %s", port_->getDevPath().c_str());
            port_->closePort();
            exit(-1);
        }
    }

    void UltrasonicDriver::sensor_msg_init(sensor_msgs::Range &range_name, const char *frame_id_name)
    {
        range_name.radiation_type = sensor_msgs::Range::ULTRASOUND;
        range_name.header.frame_id = frame_id_name;
        range_name.field_of_view = 0.26;
        range_name.min_range = 0.0;
        range_name.max_range = 2.0;
    }
}
